/* Copyright 2001,2002,2003 NAH6
 * All Rights Reserved
 *
 * Parts Copyright DoD, Parts Copyright Starium
 *
 */
/* -*-C-*-
*******************************************************************************
*
* RCS:          $Id: fxpt.h,v 1.10 2003/11/22 16:34:28 itsme Exp $
* Description:  fixed point primitives
*
* (C) Copyright 1999, Starium Ltd., all rights reserved.
*
*******************************************************************************
*/

#ifndef __FXPT_H__
#define __FXPT_H__

#include "hal_types.h"

#ifdef _USE_INLINED_FXPT

// !! this cannot be 'static' - otherwise duplicate symbols
#define INLINE static inline
#define FXPT_INCLUDE_HEADER 1
//#define FXPT_INCLUDE_PROTOTYPES 1
#define FXPT_INCLUDE_IMPLEMENTATION 1
#else
#ifdef FXPT_FROM_FXPT_C 
#define INLINE
#define FXPT_INCLUDE_HEADER 1
#define FXPT_INCLUDE_PROTOTYPES 1
#define FXPT_INCLUDE_IMPLEMENTATION 1
#else
#define INLINE
#define FXPT_INCLUDE_HEADER 1
#define FXPT_INCLUDE_PROTOTYPES 1
//#define FXPT_INCLUDE_IMPLEMENTATION 1
#endif
#endif /* _USE_INLINED_FXPT */

#ifdef FXPT_INCLUDE_HEADER

typedef int16   fxpt_16;
typedef uint16  fxpt_u16;

typedef int32   fxpt_32;
typedef uint32  fxpt_u32;

typedef int64   fxpt_64;
typedef uint64  fxpt_u64;


/*
 *  Some fxpt32 notes:
 *
 *  FX32_MIN in decimal is -2147483648
 *  FX32_MAX in decimal is +2147483647
 *  -FX32_MIN overflows
 *  -(FX32_MIN+1) in decimal is +214748364 (FX32_MAX)
 *  -FX32_MAX in decimal is -2147483648 (FX32_MIN)
 */
#define FX32_SIGN (fxpt_32) 0x80000000       /* sign bit for big type */
#define FX32_MIN  (fxpt_32) 0x80000000
#define FX32_MAX  (fxpt_32) 0x7fffffff

#define FX16_SIGN (fxpt_16) 0x8000           /* sign bit for small type */
#define FX16_MIN  (fxpt_16) 0x8000
#define FX16_MAX  (fxpt_16) 0x7fff

/* convert floating point constant in the range [-1..1) to a fxpt_16 */
#define FXPT_FLOAT_TO_Q15(float_const) ((fxpt_16) ((float_const) * (FX16_MAX + 1)))
#define FXPT_FLOAT_TO_Q12(float_const) ((fxpt_16) ((double)(float_const) * (1 << 12)))

typedef fxpt_32 sine_acc_t;     /* sine accumulator     */
typedef fxpt_32 sine_incr_t;    /* sine phase increment */

#define SINE_INCR(freq, sample_rate) \
        ((sine_incr_t) ((64.0*1024*(freq)*64)/(sample_rate)))

#define ROUND_SINE_ACC(acc) \
        ((acc) >> 6)

/*
 * These are needed to deactivate debugging code in main program.
 * Debugging cannot be used without a special version of the fxpt
 * library outfitted with overflow checking code (not for production
 * use).
 */
#define FXPT_ASSERT(x,y)
#define FXPT_SET_WARNERROR(x)
#define FXPT_SET_PUNTERROR(x)
#define FXPT_PUSHSTATE(x,y,z)
#define FXPT_POPSTATE()

#endif  /* FXPT_INCLUDE_HEADER */

#ifdef FXPT_INCLUDE_PROTOTYPES

INLINE fxpt_16 fxpt_abs16 (fxpt_16 var1);
INLINE fxpt_32 fxpt_abs32 (fxpt_32 L_var1);
INLINE fxpt_32 fxpt_add32 (fxpt_32 a, fxpt_32 b);
INLINE fxpt_32 fxpt_add32_fast (fxpt_32 a, fxpt_32 b);
INLINE fxpt_32 fxpt_sub32 (fxpt_32 a, fxpt_32 b);
INLINE fxpt_32 fxpt_mult32 (fxpt_16 var1, fxpt_16 var2);
INLINE fxpt_32 fxpt_mult32_fast (fxpt_16 var1, fxpt_16 var2);
INLINE fxpt_32 fxpt_mult64_fix (fxpt_32 var1, fxpt_32 var2, int fix);
INLINE fxpt_32 fxpt_mult64_round (fxpt_32 var1, fxpt_32 var2, int fix);
INLINE fxpt_32 fxpt_mac32 (fxpt_32 L_var3, fxpt_16 var1, fxpt_16 var2);
INLINE fxpt_32 fxpt_shl32 (fxpt_32 L_var1, int var2);
INLINE fxpt_32 fxpt_shl32_fast (fxpt_32 L_var1, int var2);
INLINE fxpt_32 fxpt_shr32 (fxpt_32 L_var1, int var2);
INLINE fxpt_32 fxpt_shr32_fast (fxpt_32 L_var1, int var2);
INLINE fxpt_16 fxpt_shl16 (fxpt_16 var1, int var2);
INLINE fxpt_16 fxpt_shl16_fast(fxpt_16 var1, int var2);
INLINE fxpt_16 fxpt_shr16 (fxpt_16 var1, fxpt_16 var2);
INLINE fxpt_16 fxpt_shr16_fast(fxpt_16 var1, fxpt_16 var2);
INLINE fxpt_16 fxpt_shr16_round(fxpt_16 var1, int var2);
INLINE fxpt_16 fxpt_extract_h (fxpt_32 L_var1);
INLINE fxpt_16 fxpt_extract_l (fxpt_32 L_var1);
INLINE fxpt_32 fxpt_negate32 (fxpt_32 L_var1);
INLINE fxpt_16 fxpt_negate16 (fxpt_16 var1);
INLINE fxpt_16 fxpt_round16 (fxpt_32 L_var1);
INLINE fxpt_16 fxpt_saturate16 (fxpt_32 L_var1);
INLINE fxpt_16 fxpt_saturate16_fix (fxpt_32 L_var1, int fix );
INLINE fxpt_16 fxpt_saturate16_round (fxpt_32 L_var1, int fix );
INLINE fxpt_32 fxpt_saturate32 (fxpt_64 LL_var1);
INLINE fxpt_32 fxpt_saturate32_round(fxpt_64 LL_var1, int fix);
INLINE fxpt_32 fxpt_deposit_h (fxpt_16 var1);
INLINE fxpt_32 fxpt_deposit_l (fxpt_16 var1);
INLINE fxpt_16 fxpt_mult_round16 (fxpt_16 var1, fxpt_16 var2);
INLINE fxpt_16 fxpt_mult_round16_fast (fxpt_16 var1, fxpt_16 var2);
INLINE fxpt_16 fxpt_mult16 (fxpt_16 var1, fxpt_16 var2);
INLINE fxpt_16 fxpt_mult16_fast (fxpt_16 var1, fxpt_16 var2);
INLINE fxpt_16 fxpt_mult16_fix (fxpt_16 var1, fxpt_16 var2, int fix);
INLINE fxpt_16 fxpt_mult16_fix_fast (fxpt_16 var1, fxpt_16 var2, int fix);
INLINE fxpt_16 fxpt_mult16_round (fxpt_16 var1, fxpt_16 var2, int fix);
INLINE fxpt_16 fxpt_square16 (fxpt_16 var1);
INLINE fxpt_16 fxpt_add16 (fxpt_16 var1, fxpt_16 var2);
INLINE fxpt_16 fxpt_add16_fast (fxpt_16 var1, fxpt_16 var2);
INLINE fxpt_16 fxpt_sub16 (fxpt_16 var1, fxpt_16 var2);
INLINE fxpt_16 fxpt_sub16_fast(fxpt_16 var1, fxpt_16 var2);
INLINE fxpt_16 fxpt_mult16_q15q11 (fxpt_16 a, fxpt_16 b);
INLINE fxpt_16 fxpt_div16 (fxpt_32 num, fxpt_32 den, int fr_bits);
INLINE fxpt_32 fxpt_sine32 (fxpt_32 x);
INLINE fxpt_16 fxpt_cos16 (fxpt_16 x);
INLINE fxpt_32 fxpt_cos32 (fxpt_32 x);
INLINE fxpt_32 fxpt_mult64_round_good (fxpt_32 var1, fxpt_32 var2, int fix);

#endif /* FXPT_INCLUDE_PROTOTYPES */

#ifdef FXPT_INCLUDE_IMPLEMENTATION

/***************************************************************************
 *
 *   FUNCTION NAME: fxpt_abs16
 *
 *   PURPOSE:
 *
 *     Take the absolute value of the 16 bit input.  An input of
 *     -0x8000 results in a return value of 0x7fff.
 *
 *   INPUTS:
 *
 *     var1
 *                     16 bit short signed integer (fxpt_16) whose value
 *                     falls in the range 0xffff 8000 <= var1 <= 0x0000 7fff.
 *
 *   OUTPUTS:
 *
 *     none
 *
 *   RETURN VALUE:
 *
 *     swOut
 *                     16 bit short signed integer (fxpt_16) whose value
 *                     falls in the range
 *                     0x0000 0000 <= swOut <= 0x0000 7fff.
 *
 *   IMPLEMENTATION:
 *
 *     Take the absolute value of the 16 bit input.  An input of
 *     -0x8000 results in a return value of 0x7fff.
 *
 *   KEYWORDS: absolute value, abs
 *
 *************************************************************************/

INLINE fxpt_16
fxpt_abs16 (fxpt_16 var1)
{
  if (var1 < 0) {
    if (var1 == FX16_MIN)
      var1 = FX16_MAX;
    else
      var1 = -var1;
  }

  return var1;
}

/***************************************************************************
 *
 *   FUNCTION NAME: fxpt_abs32
 *
 *   PURPOSE:
 *
 *     Take the absolute value of the 32 bit input.  An input of
 *     -0x8000 0000 results in a return value of 0x7fff ffff.
 *
 *   INPUTS:
 *
 *     L_var1
 *                     32 bit long signed integer (fxpt_32) whose value
 *                     falls in the range
 *                     0x8000 0000 <= L_var1 <= 0x7fff ffff.
 *
 *   OUTPUTS:
 *
 *     none
 *
 *   RETURN VALUE:
 *
 *     L_Out
 *                     32 bit long signed integer (fxpt_32) whose value
 *                     falls in the range
 *                     0x8000 0000 <= L_var1 <= 0x7fff ffff.
 *
 *
 *
 *   KEYWORDS: absolute value, abs
 *
 *************************************************************************/

INLINE fxpt_32
fxpt_abs32 (fxpt_32 L_var1)
{
  if (L_var1 < 0) {
    if (L_var1 == FX32_MIN)
      L_var1 = FX32_MAX;
    else
      L_var1 = -L_var1;
  }

  return L_var1;
}


/***************************************************************************
 *
 *   FUNCTION NAME: fxpt_add32
 *
 *   PURPOSE:
 *
 *     Perform the addition of the two 32 bit input variables with
 *     saturation.
 *
 *   INPUTS:
 *
 *     L_var1
 *                     32 bit long signed integer (Longword) whose value
 *                     falls in the range
 *                     0x8000 0000 <= L_var1 <= 0x7fff ffff.
 *     L_var2
 *                     32 bit long signed integer (Longword) whose value
 *                     falls in the range
 *                     0x8000 0000 <= L_var2 <= 0x7fff ffff.
 *
 *   OUTPUTS:
 *
 *     none
 *
 *   RETURN VALUE:
 *
 *     L_Out
 *                     32 bit long signed integer (Longword) whose value
 *                     falls in the range
 *                     0x8000 0000 <= L_var1 <= 0x7fff ffff.
 *
 *   IMPLEMENTATION:
 *
 *     Perform the addition of the two 32 bit input variables with
 *     saturation.
 *
 *     L_Out = L_var1 + L_var2
 *
 *     L_Out is set to 0x7fff ffff if the operation results in an
 *     overflow.  L_Out is set to 0x8000 0000 if the operation
 *     results in an underflow.
 *
 *   KEYWORDS: add, addition
 *
 *************************************************************************/

INLINE fxpt_32
fxpt_add32 (fxpt_32 a, fxpt_32 b)
{
  fxpt_32 sum;

  sum = a + b;

  if ((a ^ b) > 0  &&  (a ^ sum) < 0) {
    if (b > 0)
      sum = FX32_MAX;
    else
      sum = FX32_MIN;
  }

  return sum;
}

INLINE fxpt_32
fxpt_add32_fast (fxpt_32 a, fxpt_32 b)
{
  return a + b;
}


/***************************************************************************
 *
 *   FUNCTION NAME: fxpt_sub32
 *
 *   PURPOSE:
 *
 *     Perform the subtraction of the two 32 bit input variables with
 *     saturation.
 *
 *   INPUTS:
 *
 *     L_var1
 *                     32 bit long signed integer (fxpt_32) whose value
 *                     falls in the range
 *                     0x8000 0000 <= L_var1 <= 0x7fff ffff.
 *     L_var2
 *                     32 bit long signed integer (fxpt_32) whose value
 *                     falls in the range
 *                     0x8000 0000 <= L_var2 <= 0x7fff ffff.
 *
 *   OUTPUTS:
 *
 *     none
 *
 *   RETURN VALUE:
 *
 *     L_Out
 *                     32 bit long signed integer (fxpt_32) whose value
 *                     falls in the range
 *                     0x8000 0000 <= L_var1 <= 0x7fff ffff.
 *
 *   IMPLEMENTATION:
 *
 *     Perform the subtraction of the two 32 bit input variables with
 *     saturation.
 *
 *     L_Out = L_var1 - L_var2
 *
 *     L_Out is set to 0x7fff ffff if the operation results in an
 *     overflow.  L_Out is set to 0x8000 0000 if the operation
 *     results in an underflow.
 *
 *   KEYWORDS: sub, subtraction
 *
 *************************************************************************/

INLINE fxpt_32
fxpt_sub32 (fxpt_32 a, fxpt_32 b)
{
  fxpt_32 diff;

  diff = a - b;

  if ((a ^ b) < 0  &&  (a ^ diff) < 0) {
    if (b < 0)
      diff = FX32_MAX;
    else
      diff = FX32_MIN;
  }

  return diff;
}


/***************************************************************************
 *
 *   FUNCTION NAME: fxpt_mult32
 *
 *   PURPOSE:
 *
 *     Perform a fractional multipy of the two 16 bit input numbers
 *     with saturation.  Output a 32 bit number.
 *
 *   INPUTS:
 *
 *     var1
 *                     16 bit short signed integer (fxpt_16) whose value
 *                     falls in the range 0xffff 8000 <= var1 <= 0x0000 7fff.
 *     var2
 *                     16 bit short signed integer (fxpt_16) whose value
 *                     falls in the range 0xffff 8000 <= var2 <= 0x0000 7fff.
 *
 *   OUTPUTS:
 *
 *     none
 *
 *   RETURN VALUE:
 *
 *     L_Out
 *                     32 bit long signed integer (fxpt_32) whose value
 *                     falls in the range
 *                     0x8000 0000 <= L_var1 <= 0x7fff ffff.
 *
 *   IMPLEMENTATION:
 *
 *     Multiply the two the two 16 bit input numbers. If the
 *     result is within this range, left shift the result by one
 *     and output the 32 bit number.  The only possible overflow
 *     occurs when var1==var2==-0x8000.  In this case output
 *     0x7fff ffff.
 *
 *   KEYWORDS: multiply, mult, mpy
 *
 *************************************************************************/

INLINE fxpt_32
fxpt_mult32 (fxpt_16 var1, fxpt_16 var2)
{
  fxpt_32 L_product;

  if (var1 == FX16_MIN && var2 == FX16_MIN)
    L_product = FX32_MAX;                 /* overflow */
  else
  {
    L_product = (fxpt_32) var1 * var2; /* integer multiply */
    L_product = L_product << 1;
  }
  return (L_product);

}


/*
 *  Same as above without overflow checking.
 */
INLINE fxpt_32
fxpt_mult32_fast (fxpt_16 var1, fxpt_16 var2)
{
  fxpt_32 L_product = (var1 * var2);
  L_product <<= 1;
  return L_product;
}


/*
 *  32*32 bit multiply with custom radix
 */
INLINE fxpt_32
fxpt_mult64_fix (fxpt_32 var1, fxpt_32 var2, int fix)
{
  fxpt_32 L_product;
  fxpt_64 LL_product;

  LL_product = ((fxpt_64) var1 * var2);

  if( fix==0 )
      return (fxpt_32)LL_product;

  if( fix>0 )
  {
      if (LL_product >= 0)
        L_product = (fxpt_32)(LL_product >> fix);
      else {
        L_product = (fxpt_32)((-LL_product) >> fix);
        L_product = -L_product;
      }
  }
  else
  {
      if (LL_product >= 0)
        L_product = ((fxpt_32)LL_product) << (-fix);
      else {
        L_product = ((fxpt_32)(-LL_product)) << (-fix);
        L_product = -L_product;
      }
  }

  return L_product;
}

/*
 *  32*32 bit multiply with rounding and custom radix
 */
INLINE fxpt_32
fxpt_mult64_round (fxpt_32 var1, fxpt_32 var2, int fix)
{
  int sign;
  fxpt_64 LL_product;
  fxpt_32 L_product;

  LL_product = ((fxpt_64) var1 * var2);

  if (fix == 0) {
    return (fxpt_32) LL_product;
  }

  /* Note sign */
  if (LL_product < 0) {
    sign = -1;
    LL_product = -LL_product;
  }
  else
    sign = 0;

  if( fix-1>0 )
      L_product = (fxpt_32) (LL_product >> (fix - 1));
  else
      L_product = (fxpt_32) (LL_product << (-fix + 1));
  /* Round */
  L_product++;
  L_product >>= 1;

  if (sign)
    return -L_product;
  else
    return L_product;
}

/***************************************************************************
 *
 *   FUNCTION NAME: fxpt_mac32
 *
 *   PURPOSE:
 *
 *     Multiply accumulate.  Fractionally multiply two 16 bit
 *     numbers together with saturation.  Add that result to the
 *     32 bit input with saturation.  Return the 32 bit result.
 *
 *   INPUTS:
 *
 *     var1
 *                     16 bit short signed integer (fxpt_16) whose value
 *                     falls in the range 0xffff 8000 <= var1 <= 0x0000 7fff.
 *     var2
 *                     16 bit short signed integer (fxpt_16) whose value
 *                     falls in the range 0xffff 8000 <= var2 <= 0x0000 7fff.
 *     L_var3
 *                     32 bit long signed integer (fxpt_32) whose value
 *                     falls in the range
 *                     0x8000 0000 <= L_var2 <= 0x7fff ffff.
 *
 *   OUTPUTS:
 *
 *     none
 *
 *   RETURN VALUE:
 *
 *     L_Out
 *                     32 bit long signed integer (fxpt_32) whose value
 *                     falls in the range
 *                     0x8000 0000 <= L_var1 <= 0x7fff ffff.
 *
 *   IMPLEMENTATION:
 *
 *     Fractionally multiply two 16 bit numbers together with
 *     saturation.  The only numbers which will cause saturation on
 *     the multiply are 0x8000 * 0x8000.
 *
 *     Add that result to the 32 bit input with saturation.
 *     Return the 32 bit result.
 *
 *     Please note that this is not a true multiply accumulate as
 *     most processors would implement it.  The 0x8000*0x8000
 *     causes and overflow for this instruction.  On most
 *     processors this would cause an overflow only if the 32 bit
 *     input added to it were positive or zero.
 *
 *   KEYWORDS: mac, multiply accumulate
 *
 *************************************************************************/
INLINE fxpt_32
fxpt_mac32 (fxpt_32 L_var3, fxpt_16 var1, fxpt_16 var2)
{
  return (fxpt_add32 (L_var3, fxpt_mult32 (var1, var2)));
}


/***************************************************************************
 *
 *   FUNCTION NAME: fxpt_shl32
 *
 *   PURPOSE:
 *
 *     Arithmetic shift left (or right).
 *     Arithmetically shift the input left by var2.   If var2 is
 *     negative then an arithmetic shift right (fxpt_shr32) of L_var1 by
 *     -var2 is performed.
 *
 *   INPUTS:
 *
 *     var2
 *                     16 bit short signed integer (fxpt_16) whose value
 *                     falls in the range 0xffff 8000 <= var2 <= 0x0000 7fff.
 *     L_var1
 *                     32 bit long signed integer (fxpt_32) whose value
 *                     falls in the range
 *                     0x8000 0000 <= L_var1 <= 0x7fff ffff.
 *   OUTPUTS:
 *
 *     none
 *
 *   RETURN VALUE:
 *
 *     L_Out
 *                     32 bit long signed integer (fxpt_32) whose value
 *                     falls in the range
 *                     0x8000 0000 <= L_var1 <= 0x7fff ffff.
 *
 *
 *   IMPLEMENTATION:
 *
 *     Arithmetically shift the 32 bit input left by var2.  This
 *     operation maintains the sign of the input number. If var2 is
 *     negative then an arithmetic shift right (fxpt_shr32) of L_var1 by
 *     -var2 is performed.  See description of fxpt_shr32 for details.
 *
 *     Equivalent to the Full-Rate GSM ">> n" operation.  Note that
 *     ANSI-C does not guarantee operation of the C ">>" or "<<"
 *     operator for negative numbers.
 *
 *   KEYWORDS: shift, arithmetic shift left,
 *
 *************************************************************************/

INLINE fxpt_32
fxpt_shl32 (fxpt_32 L_var1, int var2)
{
  fxpt_32 L_Out;

  if (var2 < 0) {
    L_Out = L_var1 >> -var2;
  }
  else {
    L_Out = L_var1 << var2;
    if ((L_Out >> var2) != L_var1)
      L_Out = (L_var1 > 0) ? FX32_MAX : FX32_MIN;
  }

  return (L_Out);
}

/*
 *   Same as above except var2 is assumed to be between 0 and 31
 *   inclusive.
 */
INLINE fxpt_32
fxpt_shl32_fast (fxpt_32 L_var1, int var2)
{
  fxpt_32 L_Out;

  L_Out = L_var1 << var2;

  if ((L_Out >> var2) != L_var1)
    L_Out = (L_var1 > 0) ? FX32_MAX : FX32_MIN;

  return (L_Out);
}

/***************************************************************************
 *
 *   FUNCTION NAME: fxpt_shr32
 *
 *   PURPOSE:
 *
 *     Arithmetic shift right (or left).
 *     Arithmetically shift the input right by var2.   If var2 is
 *     negative then an arithmetic shift left (shl) of var1 by
 *     -var2 is performed.
 *
 *   INPUTS:
 *
 *     var2
 *                     16 bit short signed integer (fxpt_16) whose value
 *                     falls in the range 0xffff 8000 <= var2 <= 0x0000 7fff.
 *     L_var1
 *                     32 bit long signed integer (fxpt_32) whose value
 *                     falls in the range
 *                     0x8000 0000 <= L_var1 <= 0x7fff ffff.
 *   OUTPUTS:
 *
 *     none
 *
 *   RETURN VALUE:
 *
 *     L_Out
 *                     32 bit long signed integer (fxpt_32) whose value
 *                     falls in the range
 *                     0x8000 0000 <= L_var1 <= 0x7fff ffff.
 *
 *
 *   IMPLEMENTATION:
 *
 *     Arithmetically shift the input right by var2.  This
 *     operation maintains the sign of the input number. If var2 is
 *     negative then an arithmetic shift left (shl) of L_var1 by
 *     -var2 is performed.  See description of fxpt_shl32 for details.
 *
 *     The input is a 32 bit number, as is the output.
 *
 *     Equivalent to the Full-Rate GSM ">> n" operation.  Note that
 *     ANSI-C does not guarantee operation of the C ">>" or "<<"
 *     operator for negative numbers.
 *
 *   KEYWORDS: shift, arithmetic shift right,
 *
 *************************************************************************/

INLINE fxpt_32
fxpt_shr32 (fxpt_32 L_var1, int var2)
{
  fxpt_32 L_Out;

  if (var2 < 0) {
    L_Out = L_var1 << -var2;
    if ((L_Out >> -var2) != L_var1)
      L_Out = (L_var1 > 0) ? FX32_MAX : FX32_MIN;
  }
  else {
    L_Out = L_var1 >> var2;
  }

  return (L_Out);
}


/*
 *   Same as above except var2 is assumed to be between 0 and 31
 *   inclusive.
 */

INLINE fxpt_32
fxpt_shr32_fast (fxpt_32 L_var1, int var2)
{
  return L_var1 >> var2;
}


/***************************************************************************
 *
 *   FUNCTION NAME: fxpt_shl16
 *
 *   PURPOSE:
 *
 *     Arithmetically shift the input left by var2.
 *
 *
 *   INPUTS:
 *
 *     var1
 *                     16 bit short signed integer (fxpt_16) whose value
 *                     falls in the range 0xffff 8000 <= var1 <= 0x0000 7fff.
 *     var2
 *                     16 bit short signed integer (fxpt_16) whose value
 *                     falls in the range 0xffff 8000 <= var2 <= 0x0000 7fff.
 *
 *   OUTPUTS:
 *
 *     none
 *
 *   RETURN VALUE:
 *
 *     swOut
 *                     16 bit short signed integer (fxpt_16) whose value
 *                     falls in the range
 *                     0xffff 8000 <= swOut <= 0x0000 7fff.
 *
 *   IMPLEMENTATION:
 *
 *     If Arithmetically shift the input left by var2.  If var2 is
 *     negative then an arithmetic shift right (shr) of var1 by
 *     -var2 is performed.  See description of shr for details.
 *     When an arithmetic shift left is performed the var2 LS bits
 *     are zero filled.
 *
 *     The only exception is if the left shift causes an overflow
 *     or underflow.  In this case the LS bits are not modified.
 *     The number returned is 0x8000 in the case of an underflow or
 *     0x7fff in the case of an overflow.
 *
 *     The shl is equivalent to the Full-Rate GSM "<< n" operation.
 *     Note that ANSI-C does not guarantee operation of the C ">>"
 *     or "<<" operator for negative numbers - it is not specified
 *     whether this shift is an arithmetic or logical shift.
 *
 *   KEYWORDS: asl, arithmetic shift left, shift
 *
 *************************************************************************/

INLINE fxpt_16
fxpt_shl16 (fxpt_16 var1, int var2)
{
  fxpt_16 swOut;
  fxpt_32 L_Out;

  if (var2 < 0) {
    swOut = var1 >> var2;
  }
  else {

    L_Out = ((fxpt_32) var1) << var2;

    swOut = (fxpt_16) L_Out;

    if (swOut != L_Out) {
      if (var1 > 0)
        swOut = FX16_MAX;
      else
        swOut = FX16_MIN;
    }
  }

  return (swOut);
}


/*
 *   Same as above except var2 is assumed to be between 0 and 15
 *   inclusive.
 */
INLINE fxpt_16
fxpt_shl16_fast(fxpt_16 var1, int var2)
{
  fxpt_16 swOut;
  fxpt_32 L_Out;

  L_Out = ((fxpt_32) var1) << var2;

  swOut = (fxpt_16) L_Out;       /* copy low portion to swOut, overflow
                                  * could have happened
                                  */
  if (swOut != L_Out)
  {
    /* overflow  */
    if (var1 > 0)
      swOut = FX16_MAX;              /* saturate */
    else
      swOut = FX16_MIN;              /* saturate */
  }

  return (swOut);
}


/***************************************************************************
 *
 *   FUNCTION NAME: fxpt_shr16
 *
 *   PURPOSE:
 *
 *     Arithmetic shift right (or left).
 *     Arithmetically shift the input right by var2.   If var2 is
 *     negative then an arithmetic shift left (shl) of var1 by
 *     -var2 is performed.
 *
 *   INPUTS:
 *
 *     var1
 *                     16 bit short signed integer (fxpt_16) whose value
 *                     falls in the range 0xffff 8000 <= var1 <= 0x0000 7fff.
 *     var2
 *                     16 bit short signed integer (fxpt_16) whose value
 *                     falls in the range 0xffff 8000 <= var2 <= 0x0000 7fff.
 *
 *   OUTPUTS:
 *
 *     none
 *
 *   RETURN VALUE:
 *
 *     swOut
 *                     16 bit short signed integer (fxpt_16) whose value
 *                     falls in the range
 *                     0xffff 8000 <= swOut <= 0x0000 7fff.
 *
 *   IMPLEMENTATION:
 *
 *     Arithmetically shift the input right by var2.  This
 *     operation maintains the sign of the input number. If var2 is
 *     negative then an arithmetic shift left (shl) of var1 by
 *     -var2 is performed.  See description of shl for details.
 *
 *     Equivalent to the Full-Rate GSM ">> n" operation.  Note that
 *     ANSI-C does not guarantee operation of the C ">>" or "<<"
 *     operator for negative numbers.
 *
 *   KEYWORDS: shift, arithmetic shift right,
 *
 *************************************************************************/

INLINE fxpt_16
fxpt_shr16 (fxpt_16 var1, fxpt_16 var2)
{
  fxpt_32 L_Out;
  fxpt_16 swOut;

  if (var2 < 0) {
    L_Out = ((fxpt_32) var1) << var2;
    swOut = (fxpt_16) L_Out;

    if (swOut != L_Out) {
      if (var1 > 0)
        swOut = FX16_MAX;
      else
        swOut = FX16_MIN;
    }
  }
  else {
    swOut = var1 >> var2;
  }

  return (swOut);
}


/*
 *   Same as above except var2 is assumed to be between 0 and 15
 *   inclusive.
 */
INLINE fxpt_16
fxpt_shr16_fast(fxpt_16 var1, fxpt_16 var2)
{
  return var1 >> var2;
}


/*
 *   Same as fxpt_shr16_fast, except rounding is performed
 */
INLINE fxpt_16
fxpt_shr16_round(fxpt_16 var1, int var2)
{
  int sign;
  fxpt_16 rOut;

  if (var2 == 0) {
    return var1;
  }

  /* Note sign */
  if (var1 < 0) {
    sign = -1;
    rOut = -var1;
  }
  else {
    sign = 0;
    rOut = var1;
  }

  /* Rounding shift */
  rOut >>= (var2 - 1);
  rOut++;
  rOut >>= 1;

  /* Handle sign */
  if (sign)
    return -rOut;
  else
    return rOut;
}


/***************************************************************************
 *
 *   FUNCTION NAME: fxpt_extract_h
 *
 *   PURPOSE:
 *
 *     Extract the 16 MS bits of a 32 bit fxpt_32.  Return the 16 bit
 *     number as a fxpt_16.  This is used as a "truncation" of a fractional
 *     number.
 *
 *   INPUTS:
 *
 *     L_var1
 *                     32 bit long signed integer (fxpt_32) whose value
 *                     falls in the range
 *                     0x8000 0000 <= L_var1 <= 0x7fff ffff.
 *
 *   OUTPUTS:
 *
 *     none
 *
 *   RETURN VALUE:
 *
 *     swOut
 *                     16 bit short signed integer (fxpt_16) whose value
 *                     falls in the range
 *                     0xffff 8000 <= swOut <= 0x0000 7fff.
 *
 *   IMPLEMENTATION:
 *
 *   KEYWORDS: assign, truncate
 *
 *************************************************************************/

INLINE fxpt_16
fxpt_extract_h (fxpt_32 L_var1)
{
  fxpt_16 var2 = (fxpt_16)(L_var1 >> 16);
  return var2;
}


/***************************************************************************
 *
 *   FUNCTION NAME: fxpt_extract_l
 *
 *   PURPOSE:
 *
 *     Extract the 16 LS bits of a 32 bit fxpt_32.  Return the 16 bit
 *     number as a fxpt_16.  The upper portion of the input fxpt_32
 *     has no impact whatsoever on the output.
 *
 *   INPUTS:
 *
 *     L_var1
 *                     32 bit long signed integer (fxpt_32) whose value
 *                     falls in the range
 *                     0x8000 0000 <= L_var1 <= 0x7fff ffff.
 *
 *   OUTPUTS:
 *
 *     none
 *
 *   RETURN VALUE:
 *
 *     swOut
 *                     16 bit short signed integer (fxpt_16) whose value
 *                     falls in the range
 *                     0xffff 8000 <= swOut <= 0x0000 7fff.
 *
 *
 *   KEYWORDS: extract, assign
 *
 *************************************************************************/

INLINE fxpt_16
fxpt_extract_l (fxpt_32 L_var1)
{
  fxpt_16 var2;

  var2 = (fxpt_16) (0x0000ffffL & L_var1);
  return (var2);
}


/***************************************************************************
 *
 *   FUNCTION NAME: fxpt_negate32
 *
 *   PURPOSE:
 *
 *     Negate the 32 bit input. 0x8000 0000's negated value is
 *     0x7fff ffff.
 *
 *   INPUTS:
 *
 *     L_var1
 *                     32 bit long signed integer (fxpt_32) whose value
 *                     falls in the range
 *                     0x8000 0000 <= L_var1 <= 0x7fff ffff.
 *
 *   OUTPUTS:
 *
 *     none
 *
 *   RETURN VALUE:
 *
 *     L_Out
 *                     32 bit long signed integer (fxpt_32) whose value
 *                     falls in the range
 *                     0x8000 0001 <= L_var1 <= 0x7fff ffff.
 *
 *   KEYWORDS: negate, negative
 *
 *************************************************************************/

INLINE fxpt_32
fxpt_negate32 (fxpt_32 L_var1)
{
  fxpt_32 L_Out;

  if (L_var1 == FX32_MIN)
    L_Out = FX32_MAX;
  else
    L_Out = -L_var1;
  return (L_Out);
}


/***************************************************************************
 *
 *   FUNCTION NAME: fxpt_negate16
 *
 *   PURPOSE:
 *
 *     Negate the 16 bit input. 0x8000's negated value is 0x7fff.
 *
 *   INPUTS:
 *
 *     var1
 *                     16 bit short signed integer (fxpt_16) whose value
 *                     falls in the range 0xffff 8000 <= var1 <= 0x0000 7fff.
 *
 *   OUTPUTS:
 *
 *     none
 *
 *   RETURN VALUE:
 *
 *     swOut
 *                     16 bit short signed integer (fxpt_16) whose value
 *                     falls in the range
 *                     0xffff 8001 <= swOut <= 0x0000 7fff.
 *
 *   KEYWORDS: negate, negative, invert
 *
 *************************************************************************/

INLINE fxpt_16
fxpt_negate16 (fxpt_16 var1)
{
  fxpt_16 swOut;

  if (var1 == FX16_MIN)
    swOut = FX16_MAX;
  else
    swOut = -var1;
  return (swOut);
}

/***************************************************************************
 *
 *   FUNCTION NAME: fxpt_round16
 *
 *   PURPOSE:
 *
 *     Round the 32 bit fxpt_32 into a 16 bit shortword with saturation.
 *
 *   INPUTS:
 *
 *     L_var1
 *                     32 bit long signed integer (fxpt_32) whose value
 *                     falls in the range
 *                     0x8000 0000 <= L_var1 <= 0x7fff ffff.
 *   OUTPUTS:
 *
 *     none
 *
 *   RETURN VALUE:
 *
 *     swOut
 *                     16 bit short signed integer (fxpt_16) whose value
 *                     falls in the range
 *                     0xffff 8000 <= swOut <= 0x0000 7fff.
 *
 *   IMPLEMENTATION:
 *
 *     Perform a two's complement round on the input fxpt_32 with
 *     saturation.
 *
 *     This is equivalent to adding 0x0000 8000 to the input.  The
 *     result may overflow due to the add.  If so, the result is
 *     saturated.  The 32 bit rounded number is then shifted down
 *     16 bits and returned as a fxpt_16.
 *
 *
 *   KEYWORDS: round
 *
 *************************************************************************/

INLINE fxpt_16
fxpt_round16 (fxpt_32 L_var1)
{
  fxpt_32 L_Prod;

  L_Prod = fxpt_add32 (L_var1, 0x00008000L); /* round MSP */
  return (fxpt_extract_h (L_Prod));
}


/***************************************************************************
 *
 *   FUNCTION NAME: fxpt_saturate16
 *
 *   PURPOSE:
 *
 *     Limit the 32 bit input to the range of a 16 bit word.
 *
 *
 *   INPUTS:
 *
 *     L_var1
 *                     32 bit long signed integer (fxpt_32) whose value
 *                     falls in the range
 *                     0x8000 0000 <= L_var1 <= 0x7fff ffff.
 *
 *   OUTPUTS:
 *
 *     none
 *
 *   RETURN VALUE:
 *
 *     swOut
 *                     16 bit short signed integer (fxpt_16) whose value
 *                     falls in the range
 *                     0xffff 8000 <= swOut <= 0x0000 7fff.
 *
 *   KEYWORDS: saturation, limiting, limit, saturate, 16 bits
 *
 *************************************************************************/
INLINE fxpt_16
fxpt_saturate16 (fxpt_32 L_var1)
{
  fxpt_16 swOut = (fxpt_16) L_var1;

  if( swOut != L_var1 )
    swOut = (L_var1 < 0) ? FX16_MIN : FX16_MAX;

  return swOut;
}

INLINE fxpt_16
fxpt_saturate16_fix (fxpt_32 L_var1, int fix )
{
    fxpt_32 tmp;
    fxpt_16 swOut;

    if( L_var1>=0 )
        tmp = (L_var1>>fix);
    else
        tmp= -((-L_var1)>>fix);

  swOut = (fxpt_16) tmp;

  if( swOut != tmp )
    swOut = (tmp < 0) ? FX16_MIN : FX16_MAX;

  return swOut;
}

INLINE fxpt_16
fxpt_saturate16_round (fxpt_32 L_var1, int fix )
{
    fxpt_16 swOut;

    if( L_var1>=0 )
        L_var1 = ((L_var1>>(fix-1))+1)>>1;
    else
        L_var1= -((((-L_var1)>>(fix-1))+1)>>1);

  swOut = (fxpt_16) L_var1;

  if( swOut != L_var1 )
    swOut = (L_var1 < 0) ? FX16_MIN : FX16_MAX;

  return swOut;
}

INLINE fxpt_32
fxpt_saturate32 (fxpt_64 LL_var1)
{
  fxpt_32 swOut = (fxpt_32) LL_var1;

  if( swOut != LL_var1 )
    swOut = (LL_var1 < 0) ? FX32_MIN : FX32_MAX;

  return swOut;
}

INLINE fxpt_32
fxpt_saturate32_round(fxpt_64 LL_var1, int fix)
{
    fxpt_32 swOut;

    if( LL_var1>=0 )
        LL_var1 = ((LL_var1>>(fix-1))+1)>>1;
    else
        LL_var1= -((((-LL_var1)>>(fix-1))+1)>>1);
    
    swOut = (fxpt_32) LL_var1;

  if( swOut != LL_var1 )
    swOut = (LL_var1 < 0) ? FX32_MIN : FX32_MAX;

  return swOut;
}


/***************************************************************************
 *
 *   FUNCTION NAME: fxpt_deposit_h
 *
 *   PURPOSE:
 *
 *     Put the 16 bit input into the 16 MSB's of the output fxpt_32.  The
 *     LS 16 bits are zeroed.
 *
 *   INPUTS:
 *
 *     var1
 *                     16 bit short signed integer (fxpt_16) whose value
 *                     falls in the range 0xffff 8000 <= var1 <= 0x0000 7fff.
 *
 *   OUTPUTS:
 *
 *     none
 *
 *   RETURN VALUE:
 *
 *     L_Out
 *                     32 bit long signed integer (fxpt_32) whose value
 *                     falls in the range
 *                     0x8000 0000 <= L_var1 <= 0x7fff 0000.
 *
 *
 *   KEYWORDS: deposit, assign, fractional assign
 *
 *************************************************************************/

INLINE fxpt_32
fxpt_deposit_h (fxpt_16 var1)
{
  fxpt_32 L_var2;

  L_var2 = (fxpt_32) var1 << 16;
  return (L_var2);
}


/***************************************************************************
 *
 *   FUNCTION NAME: fxpt_deposit_l
 *
 *   PURPOSE:
 *
 *     Put the 16 bit input into the 16 LSB's of the output fxpt_32 with
 *     sign extension i.e. the top 16 bits are set to either 0 or 0xffff.
 *
 *   INPUTS:
 *
 *     var1
 *                     16 bit short signed integer (fxpt_16) whose value
 *                     falls in the range 0xffff 8000 <= var1 <= 0x0000 7fff.
 *
 *   OUTPUTS:
 *
 *     none
 *
 *   RETURN VALUE:
 *
 *     L_Out
 *                     32 bit long signed integer (fxpt_32) whose value
 *                     falls in the range
 *                     0xffff 8000 <= L_var1 <= 0x0000 7fff.
 *
 *   KEYWORDS: deposit, assign
 *
 *************************************************************************/

INLINE fxpt_32
fxpt_deposit_l (fxpt_16 var1)
{
  return (fxpt_32)var1;
}


/***************************************************************************
 *
 *   FUNCTION NAME: fxpt_mult_round16
 *
 *   PURPOSE:
 *
 *     Perform a fractional multipy and round of the two 16 bit
 *     input numbers with saturation.
 *
 *   INPUTS:
 *
 *     var1
 *                     16 bit short signed integer (fxpt_16) whose value
 *                     falls in the range 0xffff 8000 <= var1 <= 0x0000 7fff.
 *     var2
 *                     16 bit short signed integer (fxpt_16) whose value
 *                     falls in the range 0xffff 8000 <= var2 <= 0x0000 7fff.
 *
 *   OUTPUTS:
 *
 *     none
 *
 *   RETURN VALUE:
 *
 *     swOut
 *                     16 bit short signed integer (fxpt_16) whose value
 *                     falls in the range
 *                     0xffff 8000 <= swOut <= 0x0000 7fff.
 *
 *   IMPLEMENTATION:
 *
 *     This routine is defined as the concatenation of the multiply
 *     operation and the round operation.
 *
 *     The fractional multiply (L_mult) produces a saturated 32 bit
 *     output.  This is followed by a an add of 0x0000 8000 to the
 *     32 bit result.  The result may overflow due to the add.  If
 *     so, the result is saturated.  The 32 bit rounded number is
 *     then shifted down 16 bits and returned as a fxpt_16.
 *
 *
 *   KEYWORDS: multiply and round, round, fxpt_mult_r16, mpyr
 *
 *************************************************************************/

INLINE fxpt_16
fxpt_mult_round16 (fxpt_16 var1, fxpt_16 var2)
{
  fxpt_16 swOut;

  swOut = fxpt_round16 (fxpt_mult32 (var1, var2));
  return (swOut);
}

/*
 *  Same as above without overflow checking.
 */
INLINE fxpt_16
fxpt_mult_round16_fast (fxpt_16 var1, fxpt_16 var2)
{
  fxpt_32 lw = (var1 * var2) + 0x4000;
  lw >>= 15;
  return (fxpt_16)lw;
}

/*
 * multiply, no rounding
 */
INLINE fxpt_16
fxpt_mult16 (fxpt_16 var1, fxpt_16 var2)
{
  fxpt_16 swOut;

  swOut = fxpt_extract_h (fxpt_mult32 (var1, var2));
  return (swOut);
}

/*
 * multiply, no overflow check, no rounding
 */
#ifdef DORSEY
INLINE fxpt_16
fxpt_mult16_fast (fxpt_16 var1, fxpt_16 var2)
{
  fxpt_32 lw = (var1 * var2);

  /* When you right shift -1, it stays -1.  This is problematic
   * in many situations, so we modify the behavior so that a -1
   * right shifted becomes a 0 instead.
   */
  if (lw >= 0)
    lw >>= 15;
  else {
    lw = -lw >> 15;
    lw = -lw;
  }
  return (fxpt_16)lw;
}
#else

INLINE fxpt_16
fxpt_mult16_fast (fxpt_16 var1, fxpt_16 var2)
{
  fxpt_32 lw = (var1 * var2);

  lw >>= 15;
  return (fxpt_16)lw;
}

#endif

/*
 * multiply, custom radix
 */
INLINE fxpt_16
fxpt_mult16_fix (fxpt_16 var1, fxpt_16 var2, int fix)
{
  fxpt_32 lw = (var1 * var2);

  /* When you right shift -1, it stays -1.  This is problematic
   * in many situations, so we modify the behavior so that a -1
   * right shifted becomes a 0 instead.
   */
  if (lw >= 0)
    lw >>= fix;
  else {
    lw = -lw >> fix;
    lw = -lw;
  }

  return (fxpt_16)lw;
}

INLINE fxpt_16
fxpt_mult16_fix_fast (fxpt_16 var1, fxpt_16 var2, int fix)
{
  fxpt_32 lw = (var1 * var2);

  /* When you right shift -1, it stays -1.  This is problematic
   * in many situations, so we modify the behavior so that a -1
   * right shifted becomes a 0 instead.
   */
  if (lw >= 0)
    lw >>= fix;
  else {
    lw = -lw >> fix;
    lw = -lw;
  }

  return (fxpt_16)lw;
}

/*
 * multiply with rounding, custom radix
 */
#if 0
INLINE fxpt_16
fxpt_mult16_round (fxpt_16 var1, fxpt_16 var2, fxpt_16 fix)
{
  int sign;
  fxpt_32 lw = (var1 * var2);

  if (fix == 0) {
    return lw;
  }

  /* Note sign */
  if (lw < 0) {
    sign = -1;
    lw = -lw;
  }
  else
    sign = 0;

  lw >>= (fix - 1);
  /* Round */
  lw++;
  lw >>= 1;

  /* Sign extend if necessary */
  if (sign)
    return (fxpt_16)(-lw);
  else
    return (fxpt_16)lw;
}
#endif

INLINE fxpt_16
fxpt_mult16_round (fxpt_16 var1, fxpt_16 var2, int fix)
{
  fxpt_32 lw = (var1 * var2);

  if (fix == 0) {
    return (fxpt_16)lw;
  }

  if (lw<0) {
    lw = - ( (-lw + (1<<(fix-1))) >> fix);
  }
  else {
    lw = (lw + (1<<(fix-1))) >> fix;
  }

  if (lw> FX16_MAX )
      lw= FX16_MAX;
  else if( lw< FX16_MIN)
      lw= FX16_MIN;

  return (fxpt_16)lw;
}

INLINE fxpt_16
fxpt_square16 (fxpt_16 var1)
{
  if (var1 == FX16_MIN)
    return FX16_MAX;

  return fxpt_mult16_fast (var1, var1);
}

/***************************************************************************
 *
 *   FUNCTION NAME: fxpt_add16
 *
 *   PURPOSE:
 *
 *     Perform the addition of the two 16 bit input variable with
 *     saturation.
 *
 *   INPUTS:
 *
 *     var1
 *                     16 bit short signed integer (fxpt_16) whose value
 *                     falls in the range 0xffff 8000 <= var1 <= 0x0000 7fff.
 *     var2
 *                     16 bit short signed integer (fxpt_16) whose value
 *                     falls in the range 0xffff 8000 <= var2 <= 0x0000 7fff.
 *
 *   OUTPUTS:
 *
 *     none
 *
 *   RETURN VALUE:
 *
 *     swOut
 *                     16 bit short signed integer (fxpt_16) whose value
 *                     falls in the range
 *                     0xffff 8000 <= swOut <= 0x0000 7fff.
 *
 *   IMPLEMENTATION:
 *
 *     Perform the addition of the two 16 bit input variable with
 *     saturation.
 *
 *     swOut = var1 + var2
 *
 *     swOut is set to 0x7fff if the operation results in an
 *     overflow.  swOut is set to 0x8000 if the operation results
 *     in an underflow.
 *
 *   KEYWORDS: add, addition
 *
 *************************************************************************/

INLINE fxpt_16
fxpt_add16 (fxpt_16 var1, fxpt_16 var2)
{
  fxpt_32 L_sum;
  fxpt_16 swOut;

  L_sum = (fxpt_32) var1 + var2;
  swOut = fxpt_saturate16 (L_sum);
  return (swOut);
}

/*
 *  Same as above w/o saturation.
 */
INLINE fxpt_16
fxpt_add16_fast (fxpt_16 var1, fxpt_16 var2)
{
  return var1 + var2;
}

/***************************************************************************
 *
 *   FUNCTION NAME: fxpt_sub16
 *
 *   PURPOSE:
 *
 *     Perform the subtraction of the two 16 bit input variable with
 *     saturation.
 *
 *   INPUTS:
 *
 *     var1
 *                     16 bit short signed integer (fxpt_16) whose value
 *                     falls in the range 0xffff 8000 <= var1 <= 0x0000 7fff.
 *     var2
 *                     16 bit short signed integer (fxpt_16) whose value
 *                     falls in the range 0xffff 8000 <= var2 <= 0x0000 7fff.
 *
 *   OUTPUTS:
 *
 *     none
 *
 *   RETURN VALUE:
 *
 *     swOut
 *                     16 bit short signed integer (fxpt_16) whose value
 *                     falls in the range
 *                     0xffff 8000 <= swOut <= 0x0000 7fff.
 *
 *   IMPLEMENTATION:
 *
 *     Perform the subtraction of the two 16 bit input variable with
 *     saturation.
 *
 *     swOut = var1 - var2
 *
 *     swOut is set to 0x7fff if the operation results in an
 *     overflow.  swOut is set to 0x8000 if the operation results
 *     in an underflow.
 *
 *   KEYWORDS: sub, subtraction
 *
 *************************************************************************/

INLINE fxpt_16
fxpt_sub16 (fxpt_16 var1, fxpt_16 var2)
{
  fxpt_32 L_diff;
  fxpt_16 swOut;

  L_diff = (fxpt_32) var1 - var2;
  swOut = fxpt_saturate16 (L_diff);

  return (swOut);
}

/*
 *  Same as above w/o saturation.
 */
INLINE fxpt_16
fxpt_sub16_fast(fxpt_16 var1, fxpt_16 var2)
{
  return var1 - var2;
}

/*
 * non standard radix point routines.
 */

INLINE fxpt_16
fxpt_mult16_q15q11 (fxpt_16 a, fxpt_16 b)
{
  fxpt_32       product;

  product = fxpt_mult32_fast (a, b);
  product = fxpt_shl32_fast (product, 4);
  return fxpt_extract_h (product);
}

/*
 * ----------------------------------------------------------------
 *                      non-inline functions (mostly)
 * ----------------------------------------------------------------
 */

fxpt_32 fxpt_div32 (fxpt_32 L_num, fxpt_32 L_den, int fr_bits);

/*
 * Divide two 32-bit values and store the result in a 16-bit
 * value with the specified number of fractional bits.
 */
INLINE fxpt_16
fxpt_div16 (fxpt_32 num, fxpt_32 den, int fr_bits)
{
  return fxpt_saturate16(fxpt_div32(num, den, fr_bits));
}

/*
 * given FREQ and SAMPLE_RATE, compute the sine_incr value
 * to use to generate a sine wave for freq FREQ.  This is
 * typically used with the SINE_INCR and ROUND_SINE_ACC macros to
 * generate sine waves.
 */
sine_incr_t fxpt_compute_sine_incr (int freq, int sample_rate);

fxpt_16 fxpt_sine16 (fxpt_16 x);
fxpt_32 fxpt_sine32 (fxpt_32 x);

/*
 * The following implementation is BAAAAAAD:
INLINE fxpt_32
fxpt_sine32 (fxpt_32 x)
{
  return fxpt_deposit_h(fxpt_sine16(fxpt_extract_h(x)));
}
 */

/*
 * The cos(omega) is equal to the sin(omega + 0.5) if omega is
 * measured in gradians.  Fortunately, twos complement numbers
 * overflow in precisely the correct behavior for this to
 * work properly with fixed point numbers in 0.15 format.
 */
INLINE fxpt_16
fxpt_cos16 (fxpt_16 x)
{
  return fxpt_sine16(x + 16384);
}

INLINE fxpt_32
fxpt_cos32 (fxpt_32 x)
{
  return fxpt_sine32(x + (16384 << 16));
}

/*
 * Compute the integer square root of x.
 * The value returned is the smallest y such that y*y <= x
 */
int16 integer_sqrt (int32 x);

/*
 * compute the fixed point square root of x.
 */
fxpt_16 fxpt_sqrt16 (fxpt_16 x);

/*
 * compute the log base 10 of x, where x is an integer 0 .. 32767
 * return the result as a Q12 number, e.g., 4.12
 *
 * Note, fxpt_log10_q12 (0) is defined as 0, not -32768
 */
fxpt_16 fxpt_log10_q12 (fxpt_16 x);

/*
 *  32*32 bit multiply with rounding and custom radix
 */
INLINE fxpt_32
fxpt_mult64_round_good (fxpt_32 var1, fxpt_32 var2, int fix)
{
  fxpt_64 LL_product;

  LL_product = ((fxpt_64) var1 * var2);

  if (fix == 0) {
    return fxpt_saturate32( LL_product );
  }

  return fxpt_saturate32_round( LL_product, fix );
}

#endif  /*  FXPT_INCLUDE_IMPLEMENTATION */
#endif /* _FXPT_H_ */

